#include "In2Bala.h"

In2Bala_T* In2Bala_t;

//************************************
// Method:    	CALCU_SAMA					//< SAMA运算函数
// Programmer:  duan
// Time:		2019/06/24
// Returns:   	bool						//< 成功：true；失败：false
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	float dt					//< 运算周期
// Parameter: 	bool * DI					//< 数字量输入值数组指针
// Parameter: 	bool * DO					//< 数字量输出值数组指针
// Parameter: 	bool * AI					//< 模拟量输入值数组指针
// Parameter: 	bool * AO					//< 模拟量输出值数组指针
//************************************
bool CALCU_SAMA(In2Bala)(Model_Base_t* arg, float dt, bool* DI, bool* DO, float* AI, float* AO)
{
	/****************变量定义区开始**************/


	/*<<<<<定义输入变量>>>>>*/
	float X_Value;
	float Bias;
	float HL;
	float LL;
	float TR1_Value;
	float TR2_Value;

	bool TS1_Sta;
	bool TS2_Sta;
	/*<<<<<定义输出变量>>>>>*/
	float Y1_Value;
	float Y2_Value;
	
	/*<<<<<定义中间变量>>>>>*/

	/****************变量定义区结束**************/

	NAME(In2Bala)* Obj = (NAME(In2Bala)*)arg;

	/****************变量赋值**************/
	/*<<<<<输入变量赋值>>>>>*/
	X_Value = AI[0];
	Bias = AI[1];
	HL = AI[2];
	LL = AI[3];
	TR1_Value = AI[4];
	TR2_Value = AI[5];

	TS1_Sta = DI[0];
	TS2_Sta = DI[1];

	/**************变量赋值结束************/

	/********************************************/
	/**************算法功能实现开始**************/
	/********************************************/
	if (TS1_Sta == true && TS2_Sta == true)							//< 两路都处于跟踪状态								
	{
		Y1_Value = TR1_Value;
		Y2_Value = TR2_Value;
	}
	else if (TS1_Sta == false && TS2_Sta == false)					//< 两路都处于自动状态（不跟踪状态）
	{
		Y1_Value = X_Value + Bias;
		Y2_Value = X_Value - Bias;
	}
	else if (TS1_Sta == true && TS2_Sta == false)					//< 1路跟踪状态，2路自动状态
	{
		Y1_Value = TR1_Value;
		Y2_Value = 2 * X_Value - TR1_Value;
	}
	else if (TS1_Sta == false && TS2_Sta == true)					//< 1路自动状态，2路跟踪状态
	{
		Y1_Value = 2 * X_Value - TR2_Value;
		Y2_Value = TR2_Value;
	}

	if (Y1_Value > HL)
		Y1_Value = HL;
	if (Y1_Value < LL)
		Y1_Value = LL;

	if (Y2_Value > HL)
		Y2_Value = HL;
	if (Y2_Value < LL)
		Y2_Value = LL;

	/********************************************/
	/**************算法功能实现结束**************/
	/********************************************/
	
	/****************运算结果输出**************/
	AO[0] = Y1_Value;
	AO[1] = Y2_Value;

	/**************运算结果输出完毕************/
	return true;
}

